# 加深整数的刻度线
import cv2
import time
import numpy as np
from camera import Camera, ESC_KEY
import sys
from PIL import Image
sys.path.append("yolox")
# print(sys.path)
from yolo import YOLO

# 传入参数解释:
# frame:图像原图像
# cam_height:相机架设高度
# fx,fy:x和y方向上的焦距
# dis_grid_res:实际距离为多少时，间隔一格
# line_width:线宽
# alpha:线条透明度
def add_layer(frame, cam_height, dis_grid_res, fx, fy, line_width=1, alpha=0.5):
    height, width, _ = frame.shape
    # 这里的最小分辨率表示间隔一像素的实际距离
    dis_min_res_y = 1 / fy * cam_height
    dis_min_res_x = 1 / fx * cam_height
    # print(f"dis_min_res_y is {dis_min_res_y}")
    # print(f"dis_min_res_x is {dis_min_res_x}")
    # 创建与原图像相同大小的全透明图像
    gradient = np.zeros((height, width, 3), dtype=np.uint8)
    
    # 计算并画出每个像素距离水平中线为10的倍数的线条
    for y in range(height):
        if y < height // 2:
            distance = (height / 2 - y) / fy * cam_height
            if distance % dis_grid_res < dis_min_res_y:
                red_value = int(255 * (1 - (2 * y / height)) * alpha)
                green_value = int(255 * (2 * y / height) * alpha)
                cv2.line(gradient, (0, y), (width, y), (0, green_value, red_value), line_width)
                cv2.putText(gradient, str(int(distance//10*10)), (5, y), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1, cv2.LINE_AA)
        else:
            distance = (y - height / 2) / fy * cam_height
            if distance % dis_grid_res < dis_min_res_y:
                red_value = int(255 * (2 * (y - height // 2) / height) * alpha)
                green_value = int(255 * (1 - (2 * (y - height // 2) / height)) * alpha)
                cv2.line(gradient, (0, y), (width, y), (0, green_value, red_value), line_width)
                cv2.putText(gradient, str(int(distance//10*10)), (5, y), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1, cv2.LINE_AA)
    
    count_x = 0
    for x in range(width):
        if x < width // 2:
            distance = (width / 2 - x) / fx * cam_height
            if distance % dis_grid_res < dis_min_res_x:
                green_value = int(255 * (1 - x / width) * alpha)
                red_value = int(255 * (x / width) * alpha)
                cv2.line(gradient, (x, 0), (x, width), (0, green_value, red_value), line_width)
                cv2.putText(gradient, str(count_x*dis_grid_res), (x, height-5), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 0, 255), 1, cv2.LINE_AA)
                count_x += 1
        else:
            distance = (x - width / 2) / fx * cam_height
            if distance % dis_grid_res < dis_min_res_x:
                green_value = int(255 * (1 - x / width) * alpha)
                red_value = int(255 * (x / width) * alpha)
                cv2.line(gradient, (x, 0), (x, width), (0, green_value, red_value), line_width)
                cv2.putText(gradient, str(count_x*dis_grid_res), (x, height-5), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 0, 255), 1, cv2.LINE_AA)
                count_x += 1

    # cv2.imshow("Gradient", gradient)
    # 直接在原始帧上叠加网格线
    blended_frame = cv2.add(frame, gradient)

    return blended_frame

if __name__ == "__main__":
    camera = Camera()
    if not camera.configure_pipeline():
        exit()
    if not camera.start_pipeline():
        exit()

    # mtx_dist = camera.get_rgb_cam_param()
    # fx = mtx_dist["mtx"][0][0]
    # fy = mtx_dist["mtx"][1][1]
    # cx = mtx_dist["mtx"][0][2]
    # cy = mtx_dist["mtx"][1][2]
    fx = 308.624322
    fy = 310.129135
    print(f"fx is {fx}")
    print(f"fy is {fy}")
    # print(f"cx is {cx}")
    # print(f"cy is {cy}")

    # 相机高度
    cam_height = 100 # 单位cm
    # 相机一格表示多远
    dis_grid_res = 20 # 单位cm

    yolo = YOLO()
    start_time = time.time()
    frame_count = 0
    try:
        while True:
            color_image = camera.get_frame()
            if color_image is None:
                continue
            # # 帅：进入yolo检测
            # frame = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
            # frame = Image.fromarray(np.uint8(frame))
            # frame, image_boxes = yolo.detect_image(frame)
            # frame = np.array(frame)
            # color_image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
            # cv2.imshow("Color Viewer", color_image)
            # height, width, _ = color_image.shape

            # for image_box in image_boxes:
            #     y = int((image_box[0] + image_box[2]) / 2)
            #     x = int((image_box[1] + image_box[3]) / 2)
            #     text_cxy = "*"
            #     cv2.putText(color_image,text_cxy,(int(x), int(y)),cv2.FONT_ITALIC,1.2,(0, 0, 255),3)
            #     if y > (height/2):
            #         right_dis = (y - height/2)/fy * cam_height
            #         print(f"y_right_dis is {right_dis}")
            #     else:
            #         left_dis = (height/2 - y)/fy * cam_height
            #         print(f"y_left_dis is {left_dis}")
            #     half_x = (width/2)/fx * cam_height
            #     if x < (width/2):
            #         x_dis = half_x - (width/2 - x)/fx * cam_height
            #         print(f"x_dis is {x_dis}")
            #     else:
            #         x_dis = (x - width/2)/fx * cam_height + half_x
            #         print(f"x_dis is {x_dis}")
                
            color_image = add_layer(color_image, cam_height, dis_grid_res, fx, fy)
            cv2.imshow("Color Viewer with Gradient", color_image)

            frame_count += 1
            elapsed_time = time.time() - start_time
            if elapsed_time > 1.0:
                fps = frame_count / elapsed_time
                print(f"FPS: {fps:.2f}")
                start_time = time.time()
                frame_count = 0

            key = cv2.waitKey(1)
            if key == ord("q") or key == ESC_KEY:
                break
            elif key == ord("w"):
                camera.save_frame(color_image)  # 保存当前帧到默认目录
    except KeyboardInterrupt:
        pass
    finally:
        camera.stop_pipeline()
        cv2.destroyAllWindows()
